#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64MultiArray.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sstream>

#include "CalibrationProcess.h"
//#include "detecttest.h"

#include <imagetran/Calibrationimage.h>

using namespace std;
using namespace cv;
using namespace Eigen;


static const std::string OPENCV_WINDOW = "Image window";
int CalibrationOrDetect = 0;  



class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  //image_transport::Publisher image_pub_;
  ros::Publisher image_pub_;
  CalibrationProcess calibrationprocess;
  //detecttest Detect;

 
private:
      //要发布的信息
    imagetran::Calibrationimage Pubmessage;
    //要转换回去的图像
    sensor_msgs::ImagePtr msgImage;

public:

  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
   // ros::NodeHandle n;
    image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
    &ImageConverter::imageCb, this);
    image_pub_ = nh_.advertise<imagetran::Calibrationimage>("/image_converter/output_video", 100);

    cv::namedWindow(OPENCV_WINDOW);
  }

  ~ImageConverter()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }


  void imageCb(const sensor_msgs::ImageConstPtr& msgImage)
  {

    //判断是否标定完成
    bool IsCalibrationOver = false;
    //目标识别返回的像素坐标
    //vector<vector<double> > Pixel;
    //要发布的坐标类型
    //std_msgs::Float64MultiArray val;


/**************ROS与Opencv图像转换***********************/
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msgImage, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
/******************************************************/
    switch(CalibrationOrDetect)
    {
      case 0:
          IsCalibrationOver = calibrationprocess.CollectCalibration(cv_ptr->image);
          if(IsCalibrationOver) {CalibrationOrDetect = 1;cout << "gaile"<< endl;}

          break;
      case 1:
          DealMessage(cv_ptr);
          cout << "dealle" << endl;
          //Detect.detect(calibrationprocess.CorrectPicture(cv_ptr->image, false), Pixel);
          //DealMessage(Pixel);
          break;
      default:
          break;
    }


    // Draw an example circle on the video stream
    //if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
      //cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
    
    
    // Update GUI Window
    //cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    //cv::waitKey(3);

    
  }


void DealMessage(cv_bridge::CvImagePtr cv_ptr)
{

   MatrixXd HMatrix;
   Mat frame;

   frame = calibrationprocess.CorrectPicture(cv_ptr->image, false);

   imshow("jiaozheng", frame);
   waitKey(3);

   msgImage = cv_bridge::CvImage(std_msgs::Header(), "bgr8",  frame).toImageMsg();
   //msgImage = cv_bridge::CvImage(std_msgs::Header(), "bgr8",  calibrationprocess.CorrectPicture(cv_ptr->image, false)).toImageMsg();
   Pubmessage.RosImage = *msgImage;

   HMatrix = calibrationprocess.CalculateHmatrix(0, CORRPICTURE);

   for(int i = 0; i < 3; i ++)
   {
      for(int j = 0; j < 4; j++)
      {

        Pubmessage.hmatrix.push_back(HMatrix(i, j));
      }
   }

   image_pub_.publish(Pubmessage);
   Pubmessage.hmatrix.clear();
   cout << "sendover" << endl;
}
 /* void DealMessage(vector<vector<double> > &Pixel)
  {
     for(int i = 0; i < Pixel.size(); i++)
     {
        vector<double> world;
       
        cout << "Pixelx " << Pixel[i][0] << " Pixely " << Pixel[i][1] << endl;
         //cout << "2" << endl;
        world = calibrationprocess.PixelMapWorld(Pixel[i], 0);
        // cout << "3" << endl;

        cout << "Worldx " << world[1] <<  " Worldy " << world[0] << endl;

        val.data.push_back(world[1]);
        val.data.push_back(world[0]);


     }
     image_pub_.publish(val);
  }*/
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}
